﻿#include "vanjee_mini_lidar/vanjee_716mini_lidar_protocol.h"
#include "Poco/Buffer.h"
#include "Poco/BinaryWriter.h"
#include <boost/archive/binary_oarchive.hpp>
#include <spdlog/spdlog.h>

namespace vanjee_lidar
{
    Vanjee716MiniLidarProtocol::Vanjee716MiniLidarProtocol(int scan_gear, int simple_port) : scan_gear_{scan_gear},
                                                                                             freq_rotate_{25.f},
                                                                                             simple_port_{simple_port},
                                                                                             pre_frame_no_{0},
                                                                                             index_start_{0},
                                                                                             index_end_{1081}
    {
        switch (scan_gear)
        {
        case 1:
            freq_rotate_ = 15.0; // Hz
            break;
        case 2:
            freq_rotate_ = 25.0; // Hz
            break;
        default:
            spdlog::error("Invalid scan gear!");
        }

        server = new BinaryInterfaceServer(simple_port_);

        laser_datagram.angleStart = -2.35619449; // -135 degrees
        laser_datagram.angleEnd = 2.35619449;    // 135 degrees
        laser_datagram.angleInc = 0.017453 / 4;
        laser_datagram.numBeams = 1081; // [-135, 135] degrees
        laser_datagram.duration_rotate = 1.0 / freq_rotate_;
        laser_datagram.duration_beam = laser_datagram.duration_rotate / 1440;
        laser_datagram.duration_scan = laser_datagram.duration_beam * laser_datagram.numBeams;
        laser_datagram.minRange = 0;
        laser_datagram.maxRange = 30;
        laser_datagram.rangeArraySize = laser_datagram.numBeams;
        laser_datagram.ranges.resize(laser_datagram.numBeams);
        laser_datagram.intensities.resize(laser_datagram.numBeams);
        laser_datagram.hasIntensities = true;
        laser_datagram.minIntensity = 0.f;
        laser_datagram.maxIntensity = 1000.f;
        laser_datagram.intensityArraySize = laser_datagram.numBeams;

        spdlog::info("vanjee_716mini_protocl object constructed.");
        // use raw string literals (R"(...)") to define a multiline string
        spdlog::info(R"(
ClientSensorLaserDatagram: 
angleStart: {}
angleEnd: {}
angleInc: {}
numBeams: {}
duration_rotate: {}
duration_beam: {}
duration_scan: {}
minRange: {}
maxRange: {}
hasIntensities: {}
minIntensity: {}
maxIntensity: {}
)",
                     laser_datagram.angleStart,
                     laser_datagram.angleEnd,
                     laser_datagram.angleInc,
                     laser_datagram.numBeams,
                     laser_datagram.duration_rotate,
                     laser_datagram.duration_beam,
                     laser_datagram.duration_scan,
                     laser_datagram.minRange,
                     laser_datagram.maxRange,
                     laser_datagram.hasIntensities,
                     laser_datagram.minIntensity,
                     laser_datagram.maxIntensity);
    }

    Vanjee716MiniLidarProtocol::~Vanjee716MiniLidarProtocol()
    {
    }

    // bool Vanjee716MiniLidarProtocol::setConfig(vanjee_mini_lidar::vanjee_716mini_lidarConfig &new_config, uint32_t level)
    // {
    //     config_ = new_config;
    //     scan_.header.frame_id = config_.frame_id;
    //     scan_.angle_min = config_.min_ang;
    //     scan_.angle_max = config_.max_ang;
    //     scan_.range_min = config_.range_min;
    //     scan_.range_max = config_.range_max;
    //     scan_gear_ = config_.frequency_scan;

    //     scan_.angle_increment = 0.017453 / 4;
    //     if (scan_gear_ == 1) // 0.25°_15hz
    //     {
    //         scan_.time_increment = 1 / 15.00000000 / 1440;
    //     }
    //     else if (scan_gear_ == 2) // 0.25°_25hz
    //     {
    //         scan_.time_increment = 1 / 25.00000000 / 1440;
    //     }

    //     // adjust angle_min to min_ang config param
    //     index_start_ = (config_.min_ang + 2.35619449) / scan_.angle_increment;
    //     // adjust angle_max to max_ang config param
    //     index_end_ = 1081 - ((2.35619449 - config_.max_ang) / scan_.angle_increment);
    //     int samples = index_end_ - index_start_;
    //     scan_.ranges.resize(samples);
    //     scan_.intensities.resize(samples);

    //     std::cout << "frame_id:" << scan_.header.frame_id << std::endl;
    //     std::cout << "min_ang:" << scan_.angle_min << std::endl;
    //     std::cout << "max_ang:" << scan_.angle_max << std::endl;
    //     std::cout << "angle_increment:" << scan_.angle_increment << std::endl;
    //     std::cout << "time_increment:" << scan_.time_increment << std::endl;
    //     std::cout << "range_min:" << scan_.range_min << std::endl;
    //     std::cout << "range_max:" << scan_.range_max << std::endl;
    //     std::cout << "samples_per_scan:" << samples << std::endl;
    //     return true;
    // }

    bool Vanjee716MiniLidarProtocol::protocol(unsigned char *data, const int len)
    {
        if (data[13] != 0x0D)
        {
            return false;
        }

        if ((data[22] == 0x02 && data[23] == 0x02) || (data[22] == 0x02 && data[23] == 0x01))
        {
            heartstate_ = true;
            int l_n32TotalPackage = data[80];
            int l_n32PackageNo = data[81];
            unsigned int l_u32FrameNo = (data[75] << 24) + (data[76] << 16) + (data[77] << 8) + data[78];
            int l_n32PointNum = (data[83] << 8) + data[84];
            int l_n32Frequency = data[79];
            int l_n32StartIndex = 0;

            if (l_n32Frequency != scan_gear_)
            {
                std::cout << "The scan frequency does not match the one you setted!" << std::endl;
                return false;
            }

            if (pre_frame_no_ != l_u32FrameNo)
            {
                pre_frame_no_ = l_u32FrameNo;
                received_package_no_.clear();
                memset(scan_intensity_, 0, sizeof(scan_intensity_));
            }

            received_package_no_.insert(l_n32PackageNo);

            if (data[82] == 0x00) // Dist
            {
                l_n32StartIndex = 600 * (l_n32PackageNo - 1);
                for (int j = 0; j < l_n32PointNum; j++)
                {
                    scan_data_[l_n32StartIndex + j] = (((unsigned char)data[85 + j * 2]) << 8) +
                                                      ((unsigned char)data[86 + j * 2]);
                    scan_data_[l_n32StartIndex + j] /= 1000.0;
                    if (scan_data_[l_n32StartIndex + j] > laser_datagram.maxRange || scan_data_[l_n32StartIndex + j] < laser_datagram.minRange || scan_data_[l_n32StartIndex + j] == 0)
                    {
                        scan_data_[l_n32StartIndex + j] = NAN;
                    }
                }
            }
            else if (data[82] == 0x01) // intensities
            {
                l_n32StartIndex = 600 * (l_n32PackageNo - 3);
                for (int j = 0; j < l_n32PointNum; j++)
                {
                    scan_intensity_[l_n32StartIndex + j] = (((unsigned char)data[85 + j * 2]) << 8) +
                                                           ((unsigned char)data[86 + j * 2]);
                }
            }

            if (received_package_no_.size() == l_n32TotalPackage)
            {
                for (int i = index_start_; i < index_end_; i++)
                {
                    // scan_.ranges[i - index_start_] = scan_data_[i];
                    laser_datagram.ranges[i - index_start_] = scan_data_[i];
                    if (std::isnan(scan_data_[i - index_start_]))
                    {
                        // scan_.intensities[i - index_start_] = 0;
                        laser_datagram.intensities[i - index_start_] = 0.f;
                    }
                    else
                    {
                        // scan_.intensities[i - index_start_] = scan_intensity_[i];
                        laser_datagram.intensities[i - index_start_] = scan_intensity_[i];
                    }
                }

                const size_t resulting_msg_size = 2                                        // scanNum
                                                  + 5 * 8                                  // time_start, uniqueId, duration_beam, duration_scan, duration_rotate
                                                  + 6 * 4                                  // numBeams, angleStart, angleEnd, angleInc, minRange, maxRange
                                                  + 4                                      // ranges->length
                                                  + laser_datagram.rangeArraySize * 4      // ranges->elements
                                                  + 1                                      // hasIntensities
                                                  + 2 * 4                                  // minIntensity, maxIntensity
                                                  + 4                                      // intensities->length
                                                  + laser_datagram.intensityArraySize * 4; // intensities->elements
                Poco::Buffer<char> buffer(resulting_msg_size);
                Poco::MemoryBinaryWriter writer(buffer, Poco::BinaryWriter::StreamByteOrder::LITTLE_ENDIAN_BYTE_ORDER);

                laser_datagram.time_start = (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch())).count();

                writer << laser_datagram.scanNum++;
                writer << laser_datagram.time_start;
                writer << laser_datagram.uniqueId++;
                writer << laser_datagram.duration_beam;
                writer << laser_datagram.duration_scan;
                writer << laser_datagram.duration_rotate;
                writer << laser_datagram.numBeams;
                writer << laser_datagram.angleStart;
                writer << laser_datagram.angleEnd;
                writer << laser_datagram.angleInc;
                writer << laser_datagram.minRange;
                writer << laser_datagram.maxRange;
                writer << laser_datagram.rangeArraySize;

                for (const auto &range : laser_datagram.ranges)
                {
                    writer << (std::isnan(range) ? -1e4f : range);
                }
                spdlog::debug("Ranges: \n{}", fmt::join(laser_datagram.ranges, ", "));

                writer << static_cast<char>(laser_datagram.hasIntensities);
                writer << laser_datagram.minIntensity;
                writer << laser_datagram.maxIntensity;
                writer << laser_datagram.intensityArraySize;

                for (const auto &intensity : laser_datagram.intensities)
                {
                    writer << intensity;
                }
                spdlog::debug("Intensities: \n{}", fmt::join(laser_datagram.intensities, ", "));

                writer.flush();

                if (resulting_msg_size != buffer.size())
                    std::cerr << "Message sizes mismatch!" << std::endl;

                // std::cout << laser_datagram.ranges.data() << "\n";
                // server.write(laser_datagram.begin(), laser_datagram.size());
                server->write(buffer.begin(), buffer.size());
                // p_buffer = &buffer;
                return true;
            }
        }
        return false;
    }
}
